/*! \file	extractor.cpp
 *  \brief	Definitions for the extractor node.
*/

#include "extractor.hpp"

int main(int argc, char** argv) {
	

	ROS_INFO("Node launched.");
	
	ros::init(argc, argv, "extractor");
	
	ros::NodeHandle private_node_handle("~");
	
	extractorData startupData;
	
	startupData.read_addr = argv[0];
	startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-11);
		
	bool inputIsValid = startupData.obtainStartingData(private_node_handle);
	
	if (!inputIsValid) {
		ROS_ERROR("Configuration invalid.");
	}
		
	ROS_INFO("Startup data processed.");
	
	
	ros::NodeHandle nh;
	
	extractorNode extractor_node(nh, startupData);
	
	ROS_INFO("Node configured.");
	
	
	while (extractor_node.isStillListening()) {	
		extractor_node.extractBagFile();
	}
	
	return 0;
	
}

bool extractorData::obtainStartingData(ros::NodeHandle& nh) {
	
	wantsDepth = false;
	wantsRgb = false;
	wantsThermal = false;
	
	rawTimeFormat = true;
	
	nh.param<bool>("setupForMapping", setupForMapping, false);
	
	nh.param<bool>("splitBags", splitBags, false);
	
	nh.param<std::string>("bagSource", bagSource, "bagSource");
	nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
	
	if (outputFolder == "outputFolder") {
		if (splitBags) {
			outputFolder = bagSource.substr(0, bagSource.size());
		} else {
			outputFolder = bagSource.substr(0, bagSource.size()-4);
		}
	}
	
	outputFolder = outputFolder + "_data";
	
	nh.param<int>("maxFrames", maxFrames, 10000);
	
	nh.param<bool>("debugMode", debugMode, false);
	nh.param<bool>("verboseMode", verboseMode, false);
	
	nh.param<std::string>("rgbTopic", rgbTopic, "rgb");
	nh.param<std::string>("depthTopic", depthTopic, "depth");
	nh.param<std::string>("thermalTopic", thermalTopic, "thermal");
	
	if (rgbTopic != "rgb") {
		rgbInfoTopic = rgbTopic.substr(0, rgbTopic.find_last_of("/") + 1) + "camera_info";
		if (verboseMode) { ROS_INFO("Assuming rgbInfoTopic is (%s)", rgbInfoTopic.c_str()); }
		wantsRgb = true;
	}
	
	if (thermalTopic != "thermal") {
		thermalInfoTopic = thermalTopic.substr(0, thermalTopic.find_last_of("/") + 1) + "camera_info";
		if (verboseMode) { ROS_INFO("Assuming thermalInfoTopic is (%s)", thermalInfoTopic.c_str()); }
		wantsThermal = true;
	}
	
	if (depthTopic != "depth") {
		depthInfoTopic = depthTopic.substr(0, depthTopic.find_last_of("/") + 1) + "camera_info";
		if (verboseMode) { ROS_INFO("Assuming depthInfoTopicrosc is (%s)", depthInfoTopic.c_str()); }
		wantsDepth = true;
	}
	
	nh.param<std::string>("thermalIntrinsics", thermalIntrinsics, "thermalIntrinsics");
	nh.param<bool>("undistortThermal", undistortThermal, true);


	if (undistortThermal) {
		if (thermalIntrinsics != "thermalIntrinsics") {
			ROS_INFO("Thermal intrinsics at (%s) selected.", thermalIntrinsics.c_str());
		} else {
			ROS_ERROR("read_addr = (%s)", read_addr.c_str());
			thermalIntrinsics = read_addr + "data/calibration/csiro-aslab/miricle-1.yml";
			ROS_ERROR("No thermal intrinsics supplied. Defaulting to (%s)", thermalIntrinsics.c_str());
		}
	}
	
	nh.param<bool>("internalTimestamp", internalTimestamp, false);
	nh.param<bool>("thermistorReading", thermistorReading, false);
	nh.param<bool>("recordDuplicates", recordDuplicates, false);
	
	return true;
}


bool extractorNode::isStillListening() {
		return stillListening;
}


extractorNode::extractorNode(ros::NodeHandle& nh, extractorData startupData): 
  dataFile(NULL),
  timeFile(NULL),
  depthtimeFile(NULL),
  depthInternaltimeFile(NULL),
  imgtimeFile(NULL),
  imgInternaltimeFile(NULL),
  thermaltimeFile(NULL),
  thermalInternaltimeFile(NULL),
  count(0),
  filenum(0),
  skipFrames(skipFrames),
  maxFrames(maxFrames),
  d_fx(525),
  d_fy(525),
  d_cx(319.5),
  d_cy(239.5) {

	
	stillListening = true;

	
	configData = startupData;
	
	startTime = -1.0;
	
	
	
	
	  
  outputFilename = (char*) malloc(256);
  outputFilenameThermal = (char*) malloc(256);
	outputdepth = (char*) malloc(256);
	//outputcloud = (char*) malloc(256);
	

    if (configData.wantsThermal) {
		topics.push_back(startupData.thermalTopic);
		topics.push_back(startupData.thermalInfoTopic);
		if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.thermalTopic.c_str(), startupData.thermalInfoTopic.c_str()); }
	}
	
	if (configData.wantsRgb) {
		topics.push_back(startupData.rgbTopic);
		topics.push_back(startupData.rgbInfoTopic);
		if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.rgbTopic.c_str(), startupData.rgbInfoTopic.c_str()); }
	}
	
	if (configData.wantsDepth) {
		topics.push_back(startupData.depthTopic);
		topics.push_back(startupData.depthInfoTopic);
		if (configData.verboseMode) { ROS_INFO("Subscribing to (%s) and (%s)...", startupData.depthTopic.c_str(), startupData.depthInfoTopic.c_str()); }
	}
	
	

	count_rgb = 0 ; 
	count_depth = 0 ;
	count_rgbinfo = 0 ; 
	count_depthinfo = 0;
	count_thermal = 0 ;
	count_thermalinfo = 0 ;

	makeDirectories();
	
	if (configData.verboseMode) { ROS_INFO("Directories made."); }
	
	if (configData.thermistorReading) {
		thermistorLogFile = startupData.outputFolder + "/thermistor.txt";
		ROS_INFO("Opening file for thermistor data: (%s)\n", thermistorLogFile.c_str());
		ofs_thermistor_log.open(thermistorLogFile.c_str());
	}
	
	
	//timePath << resultsDir << "time.txt";  
	
	if (configData.wantsThermal) {
		thermaltimePath << startupData.outputFolder << "/" << "thermal.txt"; 
		thermaltimeFile = fopen(thermaltimePath.str().c_str(), "w");
		
		if (configData.internalTimestamp) {
			thermalInternaltimePath << startupData.outputFolder << "/" << "thermal-internal.txt"; 
			thermalInternaltimeFile = fopen(thermalInternaltimePath.str().c_str(), "w");
		}
		
		if (configData.recordDuplicates) {
			thermalDuplicatesPath << startupData.outputFolder << "/" << "thermal-duplicates.txt"; 
			thermalDuplicatesFile = fopen(thermalDuplicatesPath.str().c_str(), "w");
		}
	}
	
	if (configData.wantsRgb) {
		imgtimePath << startupData.outputFolder << "/" << "rgb.txt";		
		imgtimeFile = fopen(imgtimePath.str().c_str(), "w");
		
		if (configData.internalTimestamp) {
			imgInternaltimePath << startupData.outputFolder << "/" << "rgb-internal.txt"; 
			imgInternaltimeFile = fopen(imgInternaltimePath.str().c_str(), "w");
		}
		
	}
	
	if (configData.wantsDepth) {
		depthtimePath << startupData.outputFolder << "/" << "depth.txt";  
		depthtimeFile = fopen(depthtimePath.str().c_str(), "w");
		
		if (configData.internalTimestamp) {
			depthInternaltimePath << startupData.outputFolder << "/" << "depth-internal.txt"; 
			depthInternaltimeFile = fopen(depthInternaltimePath.str().c_str(), "w");
		}
	}
  
	char bagName[256];
	int idx = startupData.bagSource.find_last_of("/\\");
  
	sprintf(bagName, "%s", (startupData.bagSource.substr(idx+1, startupData.bagSource.length())).c_str());
  
	printf("%s << bagName = (%s)\n", __FUNCTION__, bagName);
  
	if (configData.wantsRgb) {
		fprintf(imgtimeFile,"# color images\n");
		fflush(imgtimeFile);
		fprintf(imgtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
		fflush(imgtimeFile);
		fprintf(imgtimeFile,"# timestamp filename\n");
		fflush(imgtimeFile);
		
		if (configData.internalTimestamp) {
			fprintf(imgInternaltimeFile,"# color images\n");
			fflush(imgInternaltimeFile);
			fprintf(imgInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
			fflush(imgInternaltimeFile);
			fprintf(imgInternaltimeFile,"# timestamp filename\n");
			fflush(imgInternaltimeFile);
		}
		
		
	}
	
	if (configData.wantsDepth) {
		fprintf(depthtimeFile,"# depth images\n");
		fflush(depthtimeFile);
		fprintf(depthtimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
		fflush(depthtimeFile);
		fprintf(depthtimeFile,"# timestamp filename\n");
		fflush(depthtimeFile);
		
		if (configData.internalTimestamp) {
			fprintf(depthInternaltimeFile,"# depth images\n");
			fflush(depthInternaltimeFile);
			fprintf(depthInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
			fflush(depthInternaltimeFile);
			fprintf(depthInternaltimeFile,"# timestamp filename\n");
			fflush(depthInternaltimeFile);
		}
		
	}
	
	if (configData.wantsThermal) {
		fprintf(thermaltimeFile,"# thermal images\n");
		fflush(thermaltimeFile);
		fprintf(thermaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
		fflush(thermaltimeFile);
		fprintf(thermaltimeFile,"# timestamp filename\n");
		fflush(thermaltimeFile);
		
		if (configData.internalTimestamp) {
			fprintf(thermalInternaltimeFile,"# thermal images\n");
			fflush(thermalInternaltimeFile);
			fprintf(thermalInternaltimeFile,"# file: '%s'\n", startupData.bagSource.c_str());
			fflush(thermalInternaltimeFile);
			fprintf(thermalInternaltimeFile,"# timestamp filename\n");
			fflush(thermalInternaltimeFile);
		}
		
		
		
		if (configData.recordDuplicates) {
			fprintf(thermalDuplicatesFile,"# thermal duplicates\n");
			fflush(thermalDuplicatesFile);
			fprintf(thermalDuplicatesFile,"# file: '%s'\n", startupData.bagSource.c_str());
			fflush(thermalDuplicatesFile);
			fprintf(thermalDuplicatesFile,"# (is potentially a duplicate)\n");
			fflush(thermalDuplicatesFile);
		}
	}
		
		
	if (configData.verboseMode) { ROS_INFO("Here [152]\n");}
	
	if (configData.wantsThermal) {
		if (configData.undistortThermal) {
			
			ROS_INFO("Reading in calibration data");
			
			if (configData.thermalIntrinsics[0] != '/') {
				configData.thermalIntrinsics = configData.read_addr + configData.thermalIntrinsics;
			}
			
			ROS_INFO("thermal intrinsics = %s", configData.thermalIntrinsics.c_str());
			
			
			FileStorage fs(configData.thermalIntrinsics, FileStorage::READ);
			fs["imageSize"] >> imageSize;
			fs["cameraMatrix"] >> cameraMatrix;
			fs["distCoeffs"] >> distCoeffs;
			fs["newCamMat"] >> newCamMat;
			fs.release();
			
			ROS_INFO("Calibration data read.");

			cameraSize = Size(imageSize.at<unsigned short>(0, 0), imageSize.at<unsigned short>(0, 1));
			
			ROS_INFO("Camera params determined.");
			
			initUndistortRectifyMap(cameraMatrix, distCoeffs,  Mat() , newCamMat, cameraSize, CV_32FC1, map1, map2);
		   
			
			
		}
	}
	
	if (configData.splitBags) {
		
		
		DIR * dirp;
		struct dirent * entry;
		
		dirp = opendir(configData.bagSource.c_str());
		
		if (dirp == NULL) {
			ROS_ERROR("Opening of directory (%s) failed.", configData.bagSource.c_str());
			return;
		}

		while ((entry = readdir(dirp)) != NULL) {
			if (entry->d_type == DT_REG) { // If the entry is a regular file
				splitBagNames.push_back(string(entry->d_name));
			}
		}

		closedir(dirp);

		sort(splitBagNames.begin(), splitBagNames.end());

		if(splitBagNames.size() <= 0)	{
			ROS_ERROR("File counting error.");
			return;
		}

		ROS_INFO("No. of bags in folder = %d", splitBagNames.size());
		
		for (unsigned int iii = 0; iii < splitBagNames.size(); iii++) {
			ROS_INFO("Bag(%d) = (%s)", iii, splitBagNames.at(iii).c_str());
		}
		
		ROS_INFO("Prepared for split bags (%s)", bagName);

	} else {
		ROS_INFO("Prepared for single bag (%s)", bagName);
		splitBagNames.push_back(bagName);
	}
	
}

extractorNode::~extractorNode() {
  if(dataFile)
    fclose(dataFile);
  if(timeFile)
    fclose(timeFile);
    
    if (configData.wantsRgb) {
		if(imgtimeFile) fclose(imgtimeFile);
		if(imgInternaltimeFile) fclose(imgInternaltimeFile);
	}
	
	if (configData.wantsDepth) {
		if(depthtimeFile) fclose(depthtimeFile);
		if(depthInternaltimeFile) fclose(depthInternaltimeFile);
	}
	
	if (configData.wantsThermal) {
		if(thermaltimeFile) fclose(thermaltimeFile);
		if(thermalInternaltimeFile) fclose(thermalInternaltimeFile);
		
		if (configData.recordDuplicates) {
			if(thermalDuplicatesFile) fclose(thermalDuplicatesFile);  
		}
		
	}
	
	if (configData.thermistorReading) {
		if (ofs_thermistor_log.is_open()) ofs_thermistor_log.close();
	}
    
	
}

void extractorNode::serverCallback(thermalvis::extractorConfig &config, uint32_t level) {
            
	//configData.maxFrames = config.maxFrames;
	//configData.debugMode = config.debugMode;
	// no point putting anything in here coz there is no ROS spinning...?
	
}

void extractorNode::makeDirectories() {
	
	char buffer[256];
	sprintf(buffer,"mkdir -p %s",configData.outputFolder.c_str());
	system(buffer);
	
	if (configData.setupForMapping) {
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"meshes/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"worlds/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"emeshes/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cmeshes/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"smeshes/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"poses/");
		system(buffer);
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"cloud/");
		system(buffer);
	}
	
	if (configData.wantsRgb) {
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"rgb/");
		system(buffer);
	}
	
	if (configData.wantsThermal) {
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"thermal/");
		system(buffer);
	}
	
	if (configData.wantsDepth) {
		sprintf(buffer,"mkdir -p %s/%s",configData.outputFolder.c_str(),"depth/");
		system(buffer);
	}
	
	char buffer_check[256];
	sprintf(buffer_check,"%s", &buffer[9]);
	
	if (configData.verboseMode) { ROS_INFO("Checking that last directory (%s) has been created..", buffer_check); }
	
	while (!boost::filesystem::exists( buffer_check ) ) {
		
	}
	
}

void extractorNode::extractBagFile()
{
	
	for (unsigned int zzz = 0; zzz < splitBagNames.size(); zzz++) {
		
		string currentBagName;
			
		
		if (configData.splitBags) {
			currentBagName = configData.bagSource + "/" + splitBagNames.at(zzz);
			bag[zzz].open(currentBagName, rosbag::bagmode::Read);
		} else {
			currentBagName = configData.bagSource;
			bag[zzz].open(configData.bagSource, rosbag::bagmode::Read);
		}
		
		ROS_INFO("Currently processing bag(%d) = (%s)...", zzz, currentBagName.c_str());
		
		
  
		view = new rosbag::View (bag[zzz], rosbag::TopicQuery(topics));

		uint32_t msg_size = view->size() ;
		
		if (configData.verboseMode) { ROS_INFO("the msg size is %d",msg_size); }
		
		int minCount = 0;
	 
		BOOST_FOREACH(rosbag::MessageInstance const m, *view) {
		  
		minCount = 99999;

		if (configData.wantsRgb) {
			if ((m.getTopic() == configData.rgbTopic) && ((count_rgb <= configData.maxFrames) || (configData.maxFrames == 0))) {

				//ROS_WARN("Processing RGB topic...");

				rgbImg = m.instantiate<sensor_msgs::Image>();
				sprintf(outputFilename, "%s/rgb/frame%06d.%s", configData.outputFolder.c_str(), count_rgb, "png");

				//ROS_INFO("Image goes to (%s)", outputFilename);

				// copy sensor msg Image to opencv 
				cv_bridge::CvImagePtr cv_ptr;
				try {
				cv_ptr = cv_bridge::toCvCopy(rgbImg, "bgr8");
				} catch (cv_bridge::Exception& e) {
				ROS_ERROR("cv_bridge exception: %s", e.what());
				return;
				}

				if (configData.debugMode) {
				cv::imshow("rgb", cv_ptr->image);
				cv::waitKey(1);
				}


				cv::imwrite(outputFilename,cv_ptr->image);
				++count_rgb ;

				if (count_rgb < minCount) {
				minCount = count_rgb;
				}
			}

			if ((m.getTopic() == configData.rgbInfoTopic) && ((count_rgbinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {

				//ROS_WARN("Processing RGB info topic...");




				rgbInfo = m.instantiate<sensor_msgs::CameraInfo>();
				
				if (configData.internalTimestamp) {
					uint64_t internal_time;
					memcpy(&internal_time, &rgbInfo->R[0], 8);

					uint32_t internal_sec, internal_nsec;

					internal_sec = internal_time / 1000000;
					internal_nsec = (internal_time % 1000000) * 1000;
					
					if (configData.rawTimeFormat) {
						fprintf(imgInternaltimeFile,"%ld ", internal_time);
					} else {
						fprintf(imgInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
					}
					
					
					fprintf(imgInternaltimeFile,"rgb/frame%06d.png\n", count_rgbinfo);

					fflush(imgInternaltimeFile);
				}

				//fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());

				
				if (configData.rawTimeFormat) {
					fprintf(imgtimeFile,"%ld ",rgbInfo->header.stamp.toNSec());
				} else {
					fprintf(imgtimeFile,"%010d.%06d ",rgbInfo->header.stamp.sec, rgbInfo->header.stamp.nsec/1000);
				}
				
				

				/*
				if (rgbInfo->binning_x!=0) {	
				unsigned long internalimg_time = (((unsigned long) rgbInfo->binning_y) << 32) | rgbInfo->binning_x;
				fprintf(imgtimeFile,"%ld \n",internalimg_time);
				}
				*/

				fprintf(imgtimeFile,"rgb/frame%06d.png\n", count_rgbinfo);

				fflush(imgtimeFile);

				

				++count_rgbinfo;

				if (count_rgbinfo < minCount) {
					minCount = count_rgbinfo;
				}
			}
		}
		
		
		if (configData.wantsDepth) {
			if ((m.getTopic() == configData.depthTopic) && ((count_depth <= configData.maxFrames) || (configData.maxFrames == 0))) {
				
				depthImg = m.instantiate<sensor_msgs::Image>();
				
				sprintf(outputdepth, "%s/depth/frame%06d.%s", configData.outputFolder.c_str(), count_depth, "png");

				if (startTime == -1.0) {
					startTime = depthImg->header.stamp.toSec();
				}
				
				// copy sensor msg Image to opencv 
				cv_bridge::CvImagePtr cv_ptr_depth;
				try {
					cv_ptr_depth = cv_bridge::toCvCopy(depthImg, "16UC1");
					// but says 32FC1 ... :(
					// http://answers.ros.org/question/10591/publish-kinect-depth-information-to-a-ros-topic/
					// cv_ptr_depth_ = cv_bridge::toCvCopy(depth_image_msgs, sensor_msgs::image_encodings::TYPE_32FC1);
					// cv_image = self.bridge.imgmsg_to_cv(data, "32FC1")
				} catch (cv_bridge::Exception& e) {
					ROS_ERROR("cv_bridge exception: %s", e.what());
					return;
				}

				cv::Mat rawDepth;
				rawDepth = cv::Mat(cv_ptr_depth->image);
				
				for (unsigned int iii = 0; iii < rawDepth.rows; iii++) {
					for (unsigned int jjj = 0; jjj < rawDepth.cols; jjj++) {
						if (5.0 * double(rawDepth.at<unsigned short>(iii,jjj)) > 65535.0) {
							rawDepth.at<unsigned short>(iii,jjj) = 0;
						} else {
							rawDepth.at<unsigned short>(iii,jjj) = 5 * rawDepth.at<unsigned short>(iii,jjj);
						}
						
					}
				}
				
				if (configData.debugMode) {
					cv::imshow("depth", rawDepth*5);
					cv::waitKey(1);
				}
				

				cv::imwrite(outputdepth,rawDepth);
				//fprintf(depthtimeFile,"%ld \n",depthImg->header.stamp.toNSec());
				//fflush(depthtimeFile);
				
				++count_depth;
				
				if (count_depth < minCount) {
					minCount = count_depth;
				}
			}
		
		
			if ((m.getTopic() == configData.depthInfoTopic) && ((count_depthinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
				depthInfo = m.instantiate<sensor_msgs::CameraInfo>();
				
				if (configData.internalTimestamp) {
					uint64_t internal_time;
					memcpy(&internal_time, &depthInfo->R[0], 8);
					
					uint32_t internal_sec, internal_nsec;
					
					internal_sec = internal_time / 1000000;
					internal_nsec = (internal_time % 1000000) * 1000;
					
					if (configData.rawTimeFormat) {
						fprintf(depthInternaltimeFile,"%ld ", internal_time);
					} else {
						fprintf(depthInternaltimeFile,"%010d.%09d ", internal_sec, internal_nsec);
					}
					
					
					
					fprintf(depthInternaltimeFile,"depth/frame%06d.png\n", count_depthinfo);
				
					fflush(depthInternaltimeFile);
				}

				if (configData.rawTimeFormat) {
					fprintf(depthtimeFile,"%ld ",depthInfo->header.stamp.toNSec());
				} else {
					fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
				}
				
				
				
				
				
				
				////fprintf(depthtimeFile,"%ld ",depthInfo->header.stamp.toNSec());
				//fprintf(depthtimeFile,"%010d.%06d ",depthInfo->header.stamp.sec, depthInfo->header.stamp.nsec/1000);
				//fprintf(depthInternaltimeFile,"%010d.%09d ",depthInfo->binning_x, depthInfo->binning_y);
				
				/*
				if (depthInfo->binning_x!=0) {	
					unsigned long internaldepth_time = (((unsigned long) depthInfo->binning_y) << 32) | depthInfo->binning_x;
					fprintf(depthtimeFile,"%ld \n",internaldepth_time);
				}
				*/
				
				fprintf(depthtimeFile,"depth/frame%06d.png\n", count_depthinfo);
				
				fflush(depthtimeFile);
				
				
				
				++count_depthinfo;
				
				if (count_depthinfo < minCount) {
					minCount = count_depthinfo;
				}
			}
		}

		if (configData.wantsThermal) {
			if ((m.getTopic() == configData.thermalTopic) && ((count_thermal <= configData.maxFrames) || (configData.maxFrames == 0))) {
				thermalImg = m.instantiate<sensor_msgs::Image>();

				sprintf(outputFilenameThermal, "%s/thermal/frame%06d.%s", configData.outputFolder.c_str(), count_thermal, "png");
				
				//ROS_INFO("Image goes to (%s)", outputFilenameThermal);

				// copy sensor msg Image to opencv 
				cv_bridge::CvImagePtr cv_ptr_thermal;
				try {
					cv_ptr_thermal = cv_bridge::toCvCopy(thermalImg, "bgr8");
				} catch (cv_bridge::Exception& e) {
					ROS_ERROR("cv_bridge exception: %s", e.what());
					return;
				}

				cv::Mat rawThermal, grayThermal, dispThermal, writeThermal;
				rawThermal = cv::Mat(cv_ptr_thermal->image);

				if (rawThermal.type() == CV_16UC3) {
					cv::cvtColor(rawThermal, grayThermal, CV_RGB2GRAY);
				} else {
					grayThermal = cv::Mat(rawThermal);
				}
				
				if (configData.undistortThermal) {
					remap(grayThermal, writeThermal, map1, map2, INTER_LINEAR, BORDER_CONSTANT, 0);
				} else {
					writeThermal = grayThermal;
				}
				
				if (configData.debugMode) {
					adaptiveDownsample(writeThermal, dispThermal);
					
					cv::imshow("thermal", dispThermal);
					cv::waitKey(1);			
				}

				
				cv::imwrite(outputFilenameThermal,writeThermal);
				//fprintf(thermaltimeFile,"%ld \n",thermalImg->header.stamp.toNSec());
				
				if (configData.recordDuplicates) {
					int equalityScore = -1;
				
					if ((matricesAreEqual(oldThermal, rawThermal) || (rawThermal.rows != oldThermal.rows))) {
						equalityScore = 1;
					} else {
						equalityScore = 0;
					}
					
					rawThermal.copyTo(oldThermal);
					
					fprintf(thermalDuplicatesFile,"%d\n", equalityScore);
					fflush(thermalDuplicatesFile);
				}
				
				
				
				
				//fflush(thermaltimeFile);
				++count_thermal;
				
				if (count_thermal < minCount) {
					minCount = count_thermal;
				}
			}
			
			
			if ((m.getTopic() == configData.thermalInfoTopic) && ((count_thermalinfo <= configData.maxFrames) || (configData.maxFrames == 0))) {
			  thermalInfo = m.instantiate<sensor_msgs::CameraInfo>();

				
				
				
				if (configData.internalTimestamp) {
					
					uint64_t internal_time;
					memcpy(&internal_time, &thermalInfo->R[0], 8);
				
					uint32_t internal_sec, internal_nsec;
				
					internal_sec = internal_time / 1000000;
					internal_nsec = (internal_time % 1000000) * 1000;
					
					if (configData.rawTimeFormat) {
						fprintf(thermalInternaltimeFile,"%ld ", internal_time);
					} else {
						fprintf(thermalInternaltimeFile,"%010u.%09u ", internal_sec, internal_nsec);
					}
					
					
					
					fprintf(thermalInternaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
				
					fflush(thermalInternaltimeFile);
				}
				
				if (configData.rawTimeFormat) {
					fprintf(thermaltimeFile,"%ld ",thermalInfo->header.stamp.toNSec());
				} else {
					fprintf(thermaltimeFile,"%010d.%06d ",thermalInfo->header.stamp.sec, thermalInfo->header.stamp.nsec/1000);
				}

				/*
				if (thermalInfo->binning_x!=0) {	
					unsigned long internalimg_time = (((unsigned long) thermalInfo->binning_y) << 32) | thermalInfo->binning_x;
					fprintf(thermaltimeFile,"%ld \n",internalimg_time);
				}
				*/
				
				fprintf(thermaltimeFile,"thermal/frame%06d.png\n", count_thermalinfo);
				
				fflush(thermaltimeFile);
				
				
				
				
				if (configData.thermistorReading) {
					float thermistorVal;
				
					memcpy(&thermistorVal, &thermalInfo->binning_x, 4);
					
					ofs_thermistor_log << thermistorVal << endl;
				}
				
				
				++count_thermalinfo;
				
				if (count_thermalinfo < minCount) {
					minCount = count_thermalinfo;
				}
				
				if ((count_thermalinfo > 0) && (count_thermalinfo % 100 == 0)) {
					ROS_INFO("At least (%d) sets of messages extracted..", count_thermalinfo);
				}
			}
		}

		if ((minCount >= configData.maxFrames) && (configData.maxFrames > 0)) {
			ROS_WARN("Breaking loop because frame count limit (%d) has been broken", configData.maxFrames);
			break;
		}

	  }
	  
	  bag[zzz].close();
		
	}
	
	
  
  stillListening = false;
  
}

float** extractorNode::ApplyBilateralFilter(float** data, const int width, const int height, float sigmaP, float sigmaR)
{
  float z0, z1;
  float sigmaPSq = sigmaP*sigmaP;
  float sigmaRSq = sigmaR*sigmaR;
 
  float maxSqRangeDif = .2*.2;
  //precision to 1 cm
  int precision = 100000;
  int numGausVals = precision*maxSqRangeDif;
  float gausApprox[numGausVals];

  for(int i = 0; i < numGausVals; ++i) {
    float value = (float(i)/numGausVals) * maxSqRangeDif;
    gausApprox[i] = exp(-value/sigmaRSq);
  }

  float** result = new float*[height];
  float** Wp = new float*[height];
  for(int i = 0; i < height; ++i){
    result[i] = new float[width];
    Wp[i] = new float[width];
    for(int j = 0; j < width; ++j) {
      //initiate on self, assures nan's represented by -1 remain
      result[i][j] = data[i][j];
      Wp[i][j] = 1;
    }
  }

  float pixGausDist[FILTER_DISTANCE][FILTER_DISTANCE],sqRangeDist,totGausDist;

  for(int v = 0; v < FILTER_DISTANCE; ++v)
    for(int w = 0; w < FILTER_DISTANCE; ++w){
      pixGausDist[v][w] =  exp(-(v*v +w*w)/(sigmaPSq));
    }

  //lose unfit data, apply bilateral filtering, store results in writeData
  for(int i = 0; i < height; ++i)
    for(int j = 0; j < width; ++j){
      z0 = data[i][j];
      //if the point exists
      if(z0 > 0 ){
	for(int v = 0; v < FILTER_DISTANCE; ++v)
	  for(int w = 0; w < FILTER_DISTANCE; ++w) {
	    if((v != 0 || w != 0) && (i+v)<height && (j+w)<width) {
	      z1 = data[i+v][j+w];
	      if(z1 > 0) {
		//calculate gausian point distance
		sqRangeDist = (z0-z1)*(z0-z1);
		//ranges that differ more than .5m do not provide additional information
		if(sqRangeDist < maxSqRangeDif) {
		  //realGausDist = exp(-realEDist/(sigmaRSq));
		  //totGausDist = pixGausDist[v][w]*realGausDist;
		  totGausDist = gausApprox[(int)(sqRangeDist*precision)];
		  result[i][j] += totGausDist*z1;
		  Wp[i][j] += totGausDist;
		  //gausian values of the opposite pixel will be equal
		  result[i+v][j+w] += totGausDist*z0;
		  Wp[i+v][j+w] += totGausDist;
		}
	      }
	    }
	  }
      }
    }

  for(int i = 0; i < height; ++i){
    for(int j = 0; j < width; ++j)
      result[i][j] = result[i][j]/Wp[i][j];
    delete[] Wp[i];
  }
  delete Wp;

  return result;
}
